using System;
using System.Collections ;
using System.Collections.Generic;
using Microsoft.Xna.Framework;
namespace sAstar {
    class AStarAlgorithm {

        private int[, ,] map;
        private int id;
        private Node startNode;
        private Node goalNode;

        public AStarAlgorithm(int[, ,] map, int id) {
            this.map = map;
            this.id = ID;
            goalNode = new Node(null, null, 1, 12, 14);
            startNode = new Node(null, goalNode, 1, 0, 0);
        }

        public AStarAlgorithm(int[, ,] map, int id, int x_goal, int y_goal, int x_start, int y_start) {
            this.map = map;
            this.id = ID;
            goalNode = new Node(null, null, 1, x_goal, y_goal);
            startNode = new Node(null, goalNode, 1, x_start, y_start);
        }

        public int[, ,] Map {
            get {
                return map;
            }
            set {
                map = value;
            }
        }

        public int ID {
            get {
                return id;
            }
            set {
                id = value;
            }
        }

        public Node StartNode {
            get {
                return startNode;
            }
            set {
                startNode = value;
            }
        }

        public Node GoalNode {
            get {
                return goalNode;
            }
            set {
                goalNode = value;
            }
        }

        public Vector2 GetNext(int x_goal, int y_goal, int x_start, int y_start) {
            
            goalNode = new Node(null, null, 1, x_goal, y_goal);
            startNode = new Node(null, goalNode, 1, x_start, y_start);

            List<Node> Solution = null;
            Solution = FindPath();
            if (Solution != null&& Solution.Count>=2) {
                int dx = Solution[1].x - Solution[0].x;
                int dy = Solution[1].y - Solution[0].y;

                return new Vector2(dx, dy);

            } else {
                return new Vector2(0, 0);    //Dung yen
            }   
        }

        private List<Node> FindPath() {
            bool isFound = false;
            List<Node> SolutionPathList = new List<Node>();

            //Tao 2 danh sach cac node da duyet (CLOSED) va chua duyet (OPEN)
            SortedCostNodeList OPEN = new SortedCostNodeList();
            SortedCostNodeList CLOSED = new SortedCostNodeList();


            //Ban dau them nut bat dau vao danh sach node chua duyet
            OPEN.push(startNode);

            //Trong khi danh sach chua duyet va khac rong
            while (OPEN.Count > 0) {
                //Lay 1 node ra                            
                Node node_current = OPEN.pop();

                //Kiem tra xem da den nut cuoi chua
                if (node_current.isMatch(goalNode)) {
                    goalNode.parentNode = node_current.parentNode;
                    isFound = true;
                    break;
                }

                //Toi uu hoa nut hien thoi dang xet (danh sach cac nut lien truoc trong duong di, chon ra nut cho chi phi it nhat)
                ArrayList successors = node_current.GetSuccessors(map, id);

                //Vong lap toi uu hoa
                foreach (Node node_successor in successors) {

                    //Tim xem nut nay co thuoc danh sach chua duyet hay khong (tuc la van con co the toi uu duoc)
                    int oFound = OPEN.IndexOf(node_successor);

                    //Neu co thi cap nhap lai. Toi uu khi va chi khi chi phi bo ra it hon nut hien thoi
                    if (oFound > 0) {
                        Node existing_node = OPEN.NodeAt(oFound);
                        if (existing_node.CompareTo(node_current) <= 0)
                            //Bo qua nut nay co nghia la da toi uu hoa (khong bo node do di khoi qua trinh duyet)
                            continue;
                    }


                    //Tim xe nut nay co thuoc danh sach da duyet
                    int cFound = CLOSED.IndexOf(node_successor);

                    //Neu co va chi phi bo ra it hon (toi uu hon) thi chon
                    if (cFound > 0) {
                        Node existing_node = CLOSED.NodeAt(cFound);
                        if (existing_node.CompareTo(node_current) <= 0)
                            //Bo qua nut nay co nghia la da toi uu hoa (khong bo node do di khoi qua trinh duyet)
                            continue;
                    }

                    //Loai bo nut nay di, vi khong toi uu
                    if (oFound != -1)
                        OPEN.RemoveAt(oFound);
                    if (cFound != -1)
                        CLOSED.RemoveAt(cFound);

                    //Them nut vao danh sach chua duyet de tim cac kha nang khac toi uu hon doi voi cac nut khac
                    OPEN.push(node_successor);

                }

                //Them nut hien thoi vao danh sach da duyet
                CLOSED.push(node_current);
            }

            //Neu co duong di tu node start den node goal
            if (isFound) {
                //lan nguoc lai de tim duong di ngan nhat
                Node p = goalNode;
                while (p != null) {
                    SolutionPathList.Insert(0, p);
                    p = p.parentNode;
                }
                return SolutionPathList;
            } else {
                return null;
            }
        }
    }
}